#include "device.h"


void m3508SetCurrent(P_M3508_CTRL pctrl, CAN_HandleTypeDef *hcan)
{
    static uint8_t m3508_p1data[8];
    static uint8_t m3508_p2data[8];
    static uint32_t m3508_txmailbox1;
    static uint32_t m3508_txmailbox2;

    static CAN_TxHeaderTypeDef M3508_PACK1_HDR =
    {
        0x200,
        0,
        CAN_ID_STD,
        CAN_RTR_DATA,
        8,
        DISABLE
    };

    static CAN_TxHeaderTypeDef M3508_PACK2_HDR =
    {
        0x1FF,
        0,
        CAN_ID_STD,
        CAN_RTR_DATA,
        8,
        DISABLE
    };

    int i;

    if(pctrl->pack == M3508_PACK_1_4)
    {
        for(i = 0; i <= 3; i++)    
        {
            m3508_p1data[i * 2]         =   pctrl->setcur[i] >> 8;
            m3508_p1data[i * 2 + 1]     =   pctrl->setcur[i] & 0x00ff;
        }
        HAL_CAN_AddTxMessage(hcan, &M3508_PACK1_HDR, m3508_p1data, &m3508_txmailbox1);

    }
    else
    {
        for(i = 0; i <= 3; i++)    
        {
            m3508_p2data[i * 2]         =   pctrl->setcur[i] >> 8;
            m3508_p2data[i * 2 + 1]     =   pctrl->setcur[i] & 0x00ff;
        }
        HAL_CAN_AddTxMessage(hcan, &M3508_PACK2_HDR, m3508_p2data, &m3508_txmailbox2);

    }
}

void m3508ResetFeedBack(P_M3508_FDBK pfdbk)
{
    P_M3508_FDBK_UNIT pfirst    = &pfdbk->m1;
    int i;
    for(i = 0; i <= 7; i++)
    {
        pfirst[i].fdbktime      = 0;
        pfirst[i].angle         = 0;
        pfirst[i].speed         = 0;
        pfirst[i].current       = 0;
        pfirst[i].temp          = 0;   
        pfirst[i].count         = 0;
        pfirst[i].sum_angle     = 0;
    }
}

uint8_t m3508FeedBack(P_CAN_RX pcanrx, P_M3508_FDBK pfdbk)
{
    uint16_t id = pcanrx->hdr.StdId - 0x200;
    P_M3508_FDBK_UNIT pfirst    = &pfdbk->m1;

    uint16_t last_angle = pfirst[id - 1].angle;
    
    pfirst[id - 1].fdbktime     = HAL_GetTick();
    pfirst[id - 1].angle        = (pcanrx->data[0] << 8) + pcanrx->data[1];
    pfirst[id - 1].speed        = (pcanrx->data[2] << 8) + pcanrx->data[3];
    pfirst[id - 1].current      = (pcanrx->data[4] << 8) + pcanrx->data[5];
    pfirst[id - 1].temp         = pcanrx->data[6];
    pfirst[id - 1].count        = pfirst[id - 1].count + 1;
    
    if(pfirst[id - 1].count == 1)
    {
        pfirst[id - 1].sum_angle = 0;
    }
    else
    {
        pfirst[id - 1].sum_angle += ((double)(pfirst[id - 1].angle) - (double)last_angle);
        if( (int)(pfirst[id - 1].angle) - (int)last_angle > 5000 )          //反向转了一圈
        {
            pfirst[id - 1].sum_angle -= 8192;
        }
        else if( (int)(pfirst[id - 1].angle) - (int)last_angle < -5000 )    //正向转了一圈   
        {
            pfirst[id - 1].sum_angle += 8192;
        }
    }

    return id;
}





